#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 使用前需运行：sudo chmod 777 /dev/ttyACM0 以确保串口权限

import rclpy
from rclpy.node import Node
import serial
import time
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Header

# 定义全局的十六进制数据，用于与力传感器通信
hex_data1 = bytes([0xFE, 0x04, 0x00, 0x00, 0x00, 0x01, 0x25, 0xC5])  # X轴数据请求
hex_data2 = bytes([0xFE, 0x04, 0x00, 0x01, 0x00, 0x01, 0x74, 0x05])  # Y轴数据请求
hex_data3 = bytes([0xFE, 0x04, 0x00, 0x02, 0x00, 0x01, 0x84, 0x05])  # Z轴数据请求
hex_datas = (hex_data1, hex_data2, hex_data3)

class ForceSensorReader(Node):
    def __init__(self):
        # 初始化节点，命名为"force_sensor_reader"
        super().__init__('force_sensor_reader')

        # 初始化串口
        self.ser = serial.Serial(
            port='/dev/ttyACM0',  # 串口设备文件
            baudrate=19200,        # 波特率
            timeout=0.001          # 读取超时时间
        )
        self.ser.flush()  # 清空串口缓冲区

        # 创建发布者，发布TwistStamped消息到"/force_data"话题
        self.publisher_ = self.create_publisher(TwistStamped, '/force_data', 10)

        # 创建定时器，每0.1秒调用一次read_force_data函数
        self.timer = self.create_timer(0.01, self.read_force_data)

    def read_force_data(self):
        # 创建TwistStamped消息对象
        twist_force = TwistStamped()
        twist_force.header = Header()
        twist_force.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳
        twist_force.header.frame_id = 'force_sensor'  # 设置参考框架

        try:
            # 依次读取X、Y、Z轴数据
            for i in range(3):
                self.ser.write(hex_datas[i])  # 发送请求数据的指令
                time.sleep(0.01)  # 等待传感器响应
                data = self.ser.readline()  # 读取传感器返回的数据

                # 检查数据是否有效
                if data and len(data) >= 5:
                    if i == 0:  # X轴数据
                        getFx = (data[3] << 8 | data[4])  # 将两个字节合并为一个整数
                        if getFx > 50000:  # 处理负值
                            getFx = getFx - 65535
                        twist_force.twist.linear.x = getFx / 100.0  # 转换为浮点数并赋值
                    elif i == 1:  # Y轴数据
                        getFy = (data[3] << 8 | data[4])
                        if getFy > 50000:
                            getFy = getFy - 65535
                        twist_force.twist.linear.y = getFy / 100.0
                    elif i == 2:  # Z轴数据
                        getFz = (data[3] << 8 | data[4])
                        if getFz > 50000:
                            getFz = getFz - 65535
                        twist_force.twist.linear.z = getFz / 100.0
                else:
                    self.get_logger().warn(f"Failed to read data for axis {i}")

            # 发布消息
            self.publisher_.publish(twist_force)

            # 打印接收到的消息
            self.get_logger().info(
                f"Force data: X={twist_force.twist.linear.x}, "
                f"Y={twist_force.twist.linear.y}, "
                f"Z={twist_force.twist.linear.z}"
            )

        except Exception as e:
            self.get_logger().error(f"Error reading force data: {e}")

def main(args=None):
    # 初始化ROS2
    rclpy.init(args=args)

    # 创建力传感器读取节点
    force_sensor_reader = ForceSensorReader()

    # 保持节点运行
    rclpy.spin(force_sensor_reader)

    # 销毁节点并关闭ROS2
    force_sensor_reader.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()